/* The MIT License:

Copyright (c) 2008-2010 Ivan Gagis <igagis@gmail.com>

Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:

The above copyright notice and this permission notice shall be included in
all copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
THE SOFTWARE. */

// Homepage: http://code.google.com/p/ting

/**
 * @file Vector3.hpp
 * @author Ivan Gagis <igagis@gmail.com>
 * @brief 3d and 2d vector math classes.
 */

#pragma once

#include <cmath>
#include <cstdlib>
#include <algorithm>

#ifdef DEBUG
#include <iostream>
#endif

#include "types.hpp"
#include "math.hpp"
#include "Exc.hpp"



namespace ting{

//  -== forward declarations ==-
template <typename T> class Vector2;
template <typename T> class Vector3;
template <typename T> class Vector4;
template <typename T> class Matrix4;
template <typename T> class Quaternion;

template <class T> inline const char* ParseNum(const char* str, T& out_Res);



template <> inline const char* ParseNum<int>(const char* str, int& out_Res){
	char buf[32];
	const char *p = str;
	unsigned i = 0;
	
	//allow '-' character in front of number
	if(*p == '-'){
		buf[i] = *p;
		++i;
		++p;
	}

	while(*p >= 0x30 && *p <= 0x39 && i < sizeof(buf)){
		buf[i] = *p;
		++i;
		++p;
		ASSERT(i < sizeof(buf))
	}
	buf[i] = 0;//null terminate
	
	if(i == 0)
		return 0;
	
	out_Res = atoi(buf);
	return p;
}



template <> inline const char* ParseNum<float>(const char* str, float& out_Res){
	char buf[32];
	const char *p = str;
	unsigned i = 0;

	//allow '-' character in front of number
	if(*p == '-'){
		buf[i] = *p;
		++i;
		++p;
	}

	bool pointEncountered = false;

	while(i < sizeof(buf)){
		if(*p < '0' || '9' < *p){
			if(!pointEncountered && *p == '.'){//allow one '.' character in the number
				pointEncountered = true;
			}else{
				break;
			}
		}

		buf[i] = *p;
		++i;
		++p;
		ASSERT(i < sizeof(buf))
	}
	buf[i] = 0;//null terminate
	
	if(i == 0)
		return 0;
	
	out_Res = atof(buf);
	return p;
}



/**
 * @brief 2 dimensional vector class.
 */
template <class T> class Vector2{
	friend class Vector3<T>;

	STATIC_ASSERT(sizeof(Vector2) == sizeof(T) * 2)
public:
	/**
	 * @brief 0th vector component.
	 */
	T x;

	/**
	 * @brief 1th vector component.
	 */
	T y;

	/**
	 * @brief default constructor.
	 * It does not initialize vector components.
	 * Their values are undefined right after construction.
	 */
	inline Vector2(){}

	/**
	 * @brief Create vector with given values.
	 * @param vecX - x component of the vector.
	 * @param vetY - y component of the vector.
	 */
	Vector2(T x, T y) :
			x(x), y(y)
	{}

	//NOTE: copy constructor will be generated by compiler

	/**
	 * @brief Create Vector2 from Vector3
	 * Creates a 2 dimensional vector and initializes its x and y components
	 * from x and y of given 3 dimensional vector.
	 * @param vec - 3 dimensional vector to copy x and y from.
	 */
	Vector2(const Vector3<T>& vec);

	/**
	 * @brief Access vector components.
	 * @param i - index of the component, can be 0 or 1.
	 */
	inline T& operator[](unsigned i){
		ASSERT(i < 2)
		ASSERT( &((&this->x)[0]) == &this->x)
		ASSERT( &((&this->x)[1]) == &this->y)
		return (&this->x)[i];
	}

	/**
	 * @brief Access vector components.
	 * @param i - index of the component, can be 0 or 1.
	 */
	inline const T& operator[](unsigned i)const{
		ASSERT(i < 2)
		ASSERT( &((&this->x)[0]) == &this->x)
		ASSERT( &((&this->x)[1]) == &this->y)
		return (&this->x)[i];
	}

	/**
	 * @brief Assign value of given Vector2 object.
	 * @param vec - reference to the Vector2 object to assigne value from.
	 * @return reference to this Vector2 object.
	 */
	inline Vector2& operator=(const Vector2& vec){
		this->x = vec.x;
		this->y = vec.y;
		return (*this);
	}


	/**
	 * @brief Assign value of given Vector3 object.
	 * Note, the z component of given Vector3 is ignored.
	 * @param vec - reference to the Vector3 object to assigne value from.
	 * @return reference to this Vector2 object.
	 */
	inline Vector2& operator=(const Vector3<T>& vec);

	/**
	 * @brief Add Vector2 and Vector3.
	 * Note, the z component of given Vector3 is ignored.
	 * @param vec - reference to the Vector3 object to add.
	 * @return instance of the resulting Vector2.
	 */
	inline Vector2 operator+(const Vector3<T>& vec)const;

	/**
	 * @brief Add and assign.
	 * Adds given Vector2 and this Vector2 and assigns the result to this Vector2.
	 * @param vec - reference to the Vector2 object to add.
	 * @return reference to this Vector2 object.
	 */
	inline Vector2& operator+=(const Vector2& vec){
		this->x += vec.x;
		this->y += vec.y;
		return (*this);
	}

	/**
	 * @brief Add two Vector2 vectors.
	 * @param vec - reference to the Vector2 object to add.
	 * @return instance of the resulting Vector2.
	 */
	inline Vector2 operator+(const Vector2& vec)const{
		return (Vector2(*this) += vec);
	}


	inline Vector2& operator-=(const Vector2& vec){
		this->x -= vec.x;
		this->y -= vec.y;
		return (*this);
	}

	inline Vector2 operator-(const Vector2& vec)const{
		return (Vector2(*this) -= vec);
	}

	inline Vector2 operator-(const Vector3<T>& vec)const;
	
	//unary minus
	inline Vector2 operator-()const{
		return Vector2(-this->x, -this->y);
	}

	inline Vector2& operator*=(T num){
		this->x *= num;
		this->y *= num;
		return (*this);
	}

	inline Vector2 operator*(T num)const{
		return (Vector2(*this) *= num);
	}

	//operator num * vec
	inline friend Vector2 operator*(T num, const Vector2& vec){
		return vec * num;
	}

	inline Vector2& operator/=(T num){
		ASSERT(num != 0)
		this->x /= num;
		this->y /= num;
		return (*this);
	}

	inline Vector2 operator/(T num)const{
		ASSERT(num != 0)
		return (Vector2(*this) /= num);
	}

	//dot product
	inline T operator*(const Vector2& vec)const{
		return (this->x * vec.x + this->y * vec.y);
	}

	inline bool operator==(const Vector2& vec)const{
		return this->x == vec.x && this->y == vec.y;
	}

	inline bool operator!=(const Vector2& vec)const{
		return !this->operator==(vec);
	}

	/**
	 * @brief Component-wise multiplication.
	 * Performs component-wise multiplication of two vectors.
	 * The result of such operation is also vector.
     * @param vec - vector to multiply by.
     * @return Vector resulting from component-wise multiplication.
     */
	inline Vector2 CompMul(const Vector2& vec)const{
		return Vector2(
				this->x * vec.x,
				this->y * vec.y
			);
	}


	inline bool IsZero()const{
		return (this->x == 0 && this->y == 0);
	}

	inline bool IsPositiveOrZero()const{
		return this->x >= 0 && this->y >= 0;
	}

	inline Vector2& Negate(){
		//NOTE: this is faster than // (*this) = -(*this);
		this->x = -this->x;
		this->y = -this->y;
		return (*this);
	}

	inline T MagPow2(){
		return Pow2(this->x) + Pow2(this->y);
	}

	inline T Magnitude(){
		return T( sqrt(this->MagPow2()) );
	}

	inline Vector2& Normalize(){
		ASSERT(this->Magnitude() != 0)
		return (*this) /= this->Magnitude();
	}

	inline Vector2& Scale(T value){
		return (*this) *= value;
	}

	inline Vector2& SetToZero(){
		this->x = 0;
		this->y = 0;
		return (*this);
	}

	//Angle is passed in radians
	Vector2& Rotate(T angle){
		T cosa = T(::cos(angle));
		T sina = T(::sin(angle));
		T tmp = this->x * cosa - this->y * sina;
		this->y = this->y * cosa + this->x * sina;
		this->x = tmp;
		return (*this);
	}

	Vector2 Rotation(T angle)const{
		return Vector2(*this).Rotate(angle);
	}


	/**
	 * @brief Create vector from string.
	 * Parse string of form "xxx, yyy" where xxx and yyy are decimal numbers.
	 * @param str - pointer to null-terminated string to parse.
	 * @return parsed Vector2 object.
	 * @throw ting::Exc - in case the string passed as argument is bad formed.
	 */
	static Vector2 ParseXY(const char* str){
		ASSERT(str)

		Vector2 vec;

		const char *p = str;

		//search the first number, allow first '-' character
		while((*p < 0x30 || *p > 0x39) && *p != '-'){
			if(*p == 0)
				throw Exc("Vector2::ParseXY(): no number found");
			++p;
		}
		
		p = ParseNum<T>(p, vec.x);
		if(!p)
			throw Exc("Vector2::ParseXY(): input string should start with dight");

		//search next number, allow first '-' character
		while((*p < 0x30 || *p > 0x39) && *p != '-'){
			if(*p == 0)
				throw Exc("Vector2::ParseXY(): second number not found");
			++p;
		}

		p = ParseNum<T>(p, vec.y);
		if(!p)
			throw Exc("Vector2::ParseXY(): second number parsing failed");

		return vec;
	}
	
#ifdef DEBUG  
	friend std::ostream& operator<<(std::ostream& s, const Vector2<T>& vec){
		s << "(" << vec.x << ", " << vec.y << ")";
		return s;
	}
#endif
};//~class Vector2



//===============================
//
//
//      Vector3 class
//
//
//===============================
template <class T> class Vector3{
	friend class Vector2<T>;
	friend class Matrix4<T>;

	STATIC_ASSERT(sizeof(Vector3) == sizeof(T) * 3)
public:
	T x, y, z;
	
	inline Vector3(){}//default constructor
	
	Vector3(T x, T y, T z) :
			x(x), y(y), z(z)
	{}

	//copy constructor will be generated by compiler

	Vector3(T num){
		this->operator=(num);
	}

	Vector3(const Vector2<T>& vec) :
			x(vec.x), y(vec.y), z(T(0))
	{}

	inline T& operator[](unsigned i){
		ASSERT(i < 3)
		ASSERT( &((&this->x)[0]) == &this->x)
		ASSERT( &((&this->x)[1]) == &this->y)
		ASSERT( &((&this->x)[2]) == &this->z)
		return (&this->x)[i];
	}

	inline const T& operator[](unsigned i)const{
		ASSERT(i < 3)
		ASSERT( &((&this->x)[0]) == &this->x)
		ASSERT( &((&this->x)[1]) == &this->y)
		ASSERT( &((&this->x)[2]) == &this->z)
		return (&this->x)[i];
	}

	inline Vector3& operator=(const Vector3& vec){
		this->x = vec.x;
		this->y = vec.y;
		this->z = vec.z;
		return *this;
	}

	inline Vector3& operator=(const Vector2<T>& vec);

	inline Vector3& operator=(T num){
		this->x = num;
		this->y = num;
		this->z = num;
		return (*this);
	}

	inline Vector3& operator+=(const Vector2<T>& vec);

	inline Vector3& operator+=(const Vector3& vec){
		this->x += vec.x;
		this->y += vec.y;
		this->z += vec.z;
		return (*this);
	}

	inline Vector3 operator+(const Vector3& vec)const{
		return (Vector3(*this) += vec);
	}

	inline Vector3& operator-=(const Vector3& vec){
		this->x -= vec.x;
		this->y -= vec.y;
		this->z -= vec.z;
		return *this;
	}

	inline Vector3 operator-(const Vector3& vec)const{
		return (Vector3(*this) -= vec);
	}

	//unary minus
	inline Vector3 operator-()const{
		return Vector3(*this).Negate();
	}

	inline Vector3& operator*=(T num){
		this->x *= num;
		this->y *= num;
		this->z *= num;
		return (*this);
	}

	inline Vector3 operator*(T num)const{
		return (Vector3(*this) *= num);
	}

	inline friend Vector3 operator*(T num, const Vector3& vec){
		return vec * num;
	}

	inline Vector3& operator/=(T num){
		ASSERT(num != 0)
		this->x /= num;
		this->y /= num;
		this->z /= num;
		return (*this);
	}

	inline Vector3 operator/(T num){
		ASSERT_INFO(num != 0, "Vector3::operator/(): division by 0")
		return (Vector3(*this) /= num);
	}

	//Dot product
	inline T operator*(const Vector3& vec)const{
		return this->x * vec.x +
					this->y * vec.y +
					this->z * vec.z;
	}

	/**
	 * @brief Component-wise multiplication.
	 * Performs component-wise multiplication of two vectors.
	 * The result of such operation is also vector.
     * @param vec - vector to multiply by.
     * @return Vector resulting from component-wise multiplication.
     */
	inline Vector3 CompMul(const Vector3& vec)const{
		return Vector3(
				this->x * vec.x,
				this->y * vec.y,
				this->z * vec.z
			);
	}

	//Cross product
	inline Vector3 operator%(const Vector3& vec)const{
		return Vector3(this->y * vec.z - this->z * vec.y,
					this->z * vec.x - this->x * vec.z,
					this->x * vec.y - this->y * vec.x
				);
	}

	inline bool IsZero()const{
		return (this->x == 0 && this->y == 0 && this->z == 0);
	}

	inline Vector3& Negate(){
		this->x = -this->x;
		this->y = -this->y;
		this->z = -this->z;
		return (*this);
	}

	//power 2 of the magnitude
	inline T MagPow2()const{
		return Pow2(this->x) + Pow2(this->y) + Pow2(this->z);
	}

	inline T Magnitude()const{
		return T(sqrt(this->MagPow2()));
	}

	inline Vector3& Normalize(){
		ASSERT(this->Magnitude() != 0)
		(*this) /= this->Magnitude();
		return (*this);
	}
	
	inline Vector3& SetToZero(){
		this->x = 0;
		this->y = 0;
		this->z = 0;
		return (*this);
	}

	inline Vector3& ProjectOnto(const Vector3& vec){
		ASSERT(this->MagPow2() != 0)
		(*this) = vec * (vec * (*this)) / vec.MagPow2();
		return (*this);
	}

	//rotate this vector with unit quaternion which represents a rotation
	inline Vector3<T>& Rotate(const Quaternion<T>& q);//see implemenation below


		/**
	 * @brief Create vector from string.
	 * Parse string of form "xxx, yyy, zzz" where xxx, yyy and zzz are decimal numbers.
	 * @param str - pointer to null-terminated string to parse.
	 * @return parsed Vector3 object.
	 * @throw ting::Exc - in case the string passed as argument is bad formed.
	 */
	static Vector3 ParseXYZ(const char* str){
		ASSERT(str)

		Vector3 vec;

		const char *p = str;

		//search the first number, allow first '-' character
		while((*p < 0x30 || *p > 0x39) && *p != '-'){
			if(*p == 0)
				throw Exc("Vector3::ParseXYZ(): no number found");
			++p;
		}

		p = ParseNum<T>(p, vec.x);
		if(!p)
			throw Exc("Vector3::ParseXYZ(): input string should start with dight");

		//search next number, allow first '-' character
		while((*p < 0x30 || *p > 0x39) && *p != '-'){
			if(*p == 0)
				throw Exc("Vector3::ParseXYZ(): second number not found");
			++p;
		}

		p = ParseNum<T>(p, vec.y);
		if(!p)
			throw Exc("Vector3::ParseXYZ(): second number parsing failed");

		//search next number, allow first '-' character
		while((*p < 0x30 || *p > 0x39) && *p != '-'){
			if(*p == 0)
				throw Exc("Vector3::ParseXYZ(): second number not found");
			++p;
		}

		p = ParseNum<T>(p, vec.z);
		if(!p)
			throw Exc("Vector3::ParseXYZ(): third number parsing failed");

		return vec;
	}


#ifdef DEBUG  
	friend std::ostream& operator<<(std::ostream& s, const Vector3<T>& vec){
		s << "(" << vec.x << ", " << vec.y << ", " << vec.z << ")";
		return s;
	}
#endif
};//~class Vector3



//===============================
//
//
//      Vector4 class
//
//
//===============================
template <class T> class Vector4{
public:
	T x, y, z, w;

	inline Vector4(){}//default constructor

	Vector4(T x, T y, T z, T w) :
			x(x), y(y), z(z), w(w)
	{}

	//copy constructor will be generated by compiler

	Vector4(T num){
		this->operator=(num);
	}

	Vector4(T num, T w) :
			x(num), y(num), z(num), w(w)
	{}

	Vector4(const Vector2<T>& vec, T z = 0, T w = 1) :
			x(vec.x), y(vec.y), z(z), w(w)
	{}

	Vector4(const Vector3<T>& vec, T w = 1) :
			x(vec.x), y(vec.y), z(vec.z), w(w)
	{}

	inline T& operator[](unsigned i){
		ASSERT(i < 4)
		ASSERT( &((&this->x)[0]) == &this->x)
		ASSERT( &((&this->x)[1]) == &this->y)
		ASSERT( &((&this->x)[2]) == &this->z)
		ASSERT( &((&this->x)[3]) == &this->w)
		return (&this->x)[i];
	}

	inline const T& operator[](unsigned i)const{
		ASSERT(i < 4)
		ASSERT( &((&this->x)[0]) == &this->x)
		ASSERT( &((&this->x)[1]) == &this->y)
		ASSERT( &((&this->x)[2]) == &this->z)
		ASSERT( &((&this->x)[3]) == &this->w)
		return (&this->x)[i];
	}

	inline Vector4& operator=(const Vector4& vec){
		this->x = vec.x;
		this->y = vec.y;
		this->z = vec.z;
		this->w = vec.w;
		return *this;
	}

	inline Vector4& operator=(const Vector3<T>& vec){
		this->x = vec.x;
		this->y = vec.y;
		this->z = vec.z;
		this->w = 1;
		return *this;
	}

	inline Vector4& operator=(const Vector2<T>& vec){
		this->x = vec.x;
		this->y = vec.y;
		this->z = 0;
		this->w = 1;
		return *this;
	}

	inline Vector4& operator=(T num){
		this->x = num;
		this->y = num;
		this->z = num;
		this->w = num;
		return (*this);
	}

	inline Vector4& operator+=(const Vector2<T>& vec){
		this->x += vec.x;
		this->y += vec.y;
		return *this;
	}

	inline Vector4& operator+=(const Vector3<T>& vec){
		this->x += vec.x;
		this->y += vec.y;
		this->z += vec.z;
		return *this;
	}

	inline Vector4& operator+=(const Vector4& vec){
		this->x += vec.x;
		this->y += vec.y;
		this->z += vec.z;
		this->w += vec.w;
		return *this;
	}

	inline Vector4 operator+(const Vector4& vec)const{
		return (Vector4(*this) += vec);
	}

	inline Vector4& operator-=(const Vector4& vec){
		this->x -= vec.x;
		this->y -= vec.y;
		this->z -= vec.z;
		this->w -= vec.w;
		return *this;
	}

	inline Vector4 operator-(const Vector4& vec)const{
		return (Vector4(*this) -= vec);
	}

	//unary minus
	inline Vector4 operator-()const{
		return Vector4(*this).Negate();
	}

	inline Vector4& operator*=(T num){
		this->x *= num;
		this->y *= num;
		this->z *= num;
		this->w *= num;
		return (*this);
	}

	inline Vector4 operator*(T num)const{
		return (Vector4(*this) *= num);
	}

	inline friend Vector4 operator*(T num, const Vector4& vec){
		return vec * num;
	}

	inline Vector4& operator/=(T num){
		ASSERT_INFO(num != 0, "Vector4::operator/=(): division by 0")
		this->x /= num;
		this->y /= num;
		this->z /= num;
		this->w /= num;
		return (*this);
	}

	inline Vector4 operator/(T num){
		ASSERT_INFO(num != 0, "Vector4::operator/(): division by 0")
		return (Vector4(*this) /= num);
	}

	//Dot product
	inline T operator*(const Vector4& vec)const{
		return
				this->x * vec.x +
				this->y * vec.y +
				this->z * vec.z +
				this->w * vec.w
			;
	}

	//Cross product
	inline Vector4 operator%(const Vector4& vec)const{
		return Vector4(
				this->y * vec.z - this->z * vec.y,
				this->z * vec.x - this->x * vec.z,
				this->x * vec.y - this->y * vec.x,
				this->w * vec.w //TODO:???
			);
	}

	inline Vector4& Negate(){
		this->x = -this->x;
		this->y = -this->y;
		this->z = -this->z;
		this->w = -this->w;
		return (*this);
	}

	//power 2 of the magnitude
	inline T MagPow2()const{
		return Pow2(this->x) + Pow2(this->y) + Pow2(this->z) + Pow2(this->w);
	}

	inline T Magnitude()const{
		return T(sqrt(this->MagPow2()));
	}

	inline Vector4& Normalize(){
		ASSERT(this->Magnitude() != 0)
		(*this) /= this->Magnitude();
		return (*this);
	}

#ifdef DEBUG
	friend std::ostream& operator<<(std::ostream& s, const Vector4<T>& vec){
		s << "(" << vec.x << ", " << vec.y << ", " << vec.z << ", " << vec.w << ")";
		return s;
	}
#endif
};//~class Vector4



//===============================
//
//
//      Matrix4 class
//
//
//===============================
/**
 * @brief 4x4 matrix template class.
 * Note, that this matrix class stores elements in memory column by column.
 * This is the same way as OpenGL matrices are stored in memory.
 * This means easy use of this class with OpenGL.
 */
template <typename T> class Matrix4{
	//OpenGL compatible matrix elements array, if T is float or double
	T m[4 * 4]; //matrix components 0-3 1st column, 4-7 2nd column, 8-11 3rd column, 12-15 4th column
public:


	
	/**
	 * @brief Default constructor.
	 * NOTE: it does not initialize the matrix with any values.
	 * Matrix elements are undefined after the matrix is created with this constructor.
	 */
	inline Matrix4(){}//Default constructor.



	//copy constructor must be trivial.
	//Let's allow compiler to make it for us.
	//Matrix4(const Matrix4& matr){}

	

	/**
	 * @brief returns pointer to specified column.
	 * Returns pointer to array of 4 elements which forms a matrix column specified by argument.
	 * Thus, it is possible to access matrix elements using double [] operator as follows:
	 * @code
	 * Matrix4 m;
	 * m[0][0] = 1;//assign 1 to element at row 0 column 0
	 * m[3][2] = 3;//assign 3 to element at row 2 column 3
	 * float elem = m[4][3];//assign value at row 3 column 4 of the matrix to variable 'elem'
	 * @endcode
	 * @param col - column number.
	 * @return pointer to array of 4 elements which forms the requested column of the matirx.
	 */
	inline T* operator[](unsigned col){
		ASSERT(col < 4)
		return &this->m[col * 4];
	}

	/**
	 * @brief returns pointer to specified column.
	 * Const variant of operator[].
	 * @param col - column number.
	 * @return pointer to array of 4 elements which forms the requested column of the matirx.
	 */
	inline const T* operator[](unsigned col)const{
		ASSERT(col < 4)
		return &this->m[col * 4];
	}

	/**
	 * @brief get element at given row and column.
	 * @param row - row of element.
	 * @param col - column of element.
	 * @return reference to the element at specified row and column.
	 */
	inline T& E(unsigned row, unsigned col){
		ASSERT(row < 4)
		ASSERT(col < 4)
		return this->m[col * 4 + row];
	}

	//Multiply by Vector3 (M * V). i.e. transform vector with transformation matrix
	Vector3<T> operator*(const Vector3<T>& vec)const{
		return Vector3<T>(
					this->m[0] * vec[0] + this->m[4] * vec[1] + this->m[8] * vec[2] + this->m[12],
					this->m[1] * vec[0] + this->m[5] * vec[1] + this->m[9] * vec[2] + this->m[13],
					this->m[2] * vec[0] + this->m[6] * vec[1] + this->m[10] * vec[2] + this->m[14]
				);
	}

	inline Matrix4& operator=(const Matrix4& matr){
		memcpy(this->m, matr.m, sizeof(this->m));
		return (*this);
	}



	/**
	 * @brief Transpose matrix.
	 */
	Matrix4& Transpose(){
		std::swap(this->m[1], this->m[4]);
		std::swap(this->m[2], this->m[8]);
		std::swap(this->m[6], this->m[9]);
		std::swap(this->m[3], this->m[12]);
		std::swap(this->m[7], this->m[13]);
		std::swap(this->m[11], this->m[14]);
		return (*this);
	}



	/**
	 * @brief Multipply by matrix from the right.
	 * Multiply this matrix by matrix M from the right, i.e. m  = m * M
	 * @param M - matrix to multiply by.
	 * @return reference to this matrix object.
	 */
	Matrix4& RightMultMatrix(const Matrix4 &M){
		//TODO: rewrite to use Matrix4 instead of T tmpM[16]
		T tmpM[16];
		for(unsigned i = 0; i < 4; ++i){
			tmpM[4*i]  =m[0]*M.m[4*i]+m[4]*M.m[4*i+1]+m[8]*M.m[4*i+2]+ m[12]*M.m[4*i+3];
			tmpM[4*i+1]=m[1]*M.m[4*i]+m[5]*M.m[4*i+1]+m[9]*M.m[4*i+2]+ m[13]*M.m[4*i+3];
			tmpM[4*i+2]=m[2]*M.m[4*i]+m[6]*M.m[4*i+1]+m[10]*M.m[4*i+2]+m[14]*M.m[4*i+3];
			tmpM[4*i+3]=m[3]*M.m[4*i]+m[7]*M.m[4*i+1]+m[11]*M.m[4*i+2]+m[15]*M.m[4*i+3];
		}
		memcpy(this->m, tmpM, sizeof(this->m));
		//*this=tmp;
		return (*this);
	}



	/**
	 * @brief Multipply by matrix from the left.
	 * Multiply this matrix by matrix M from the left, i.e. m  = M * m
	 * @param M - matrix to multiply by.
	 * @return reference to this matrix object.
	 */
	Matrix4& LeftMultMatrix(const Matrix4& M){
		//TODO: rewrite to use Matrix4 instead of T tmpM[16]
		T tmpM[16];
		for(unsigned i = 0; i < 4; ++i){
			tmpM[4*i]  =m[4*i]*M.m[0]+m[4*i+1]*M.m[4]+m[4*i+2]*M.m[8]+ m[4*i+3]*M.m[12];
			tmpM[4*i+1]=m[4*i]*M.m[1]+m[4*i+1]*M.m[5]+m[4*i+2]*M.m[9]+ m[4*i+3]*M.m[13];
			tmpM[4*i+2]=m[4*i]*M.m[2]+m[4*i+1]*M.m[6]+m[4*i+2]*M.m[10]+m[4*i+3]*M.m[14];
			tmpM[4*i+3]=m[4*i]*M.m[3]+m[4*i+1]*M.m[7]+m[4*i+2]*M.m[11]+m[4*i+3]*M.m[15];
		}
		memcpy(this->m, tmpM, sizeof(this->m));
		return (*this);
	}



	/**
	 * @brief Initialize this matrix with identity matrix.
	 */
	Matrix4& Identity(){
		this->m[0] = 1;    this->m[4] = 0;    this->m[8] = 0;    this->m[12] = 0;
		this->m[1] = 0;    this->m[5] = 1;    this->m[9] = 0;    this->m[13] = 0;
		this->m[2] = 0;    this->m[6] = 0;    this->m[10] = 1;   this->m[14] = 0;
		this->m[3] = 0;    this->m[7] = 0;    this->m[11] = 0;   this->m[15] = 1;
		return (*this);
	}



	/**
	 * @brief Multiply current matrix by scale matrix.
	 * Multiplies this matrix by Scale matrix from the right (M = M * S).
	 * @param scale - vector of scaling factors in x, y and z directons.
	 * @return reference to this Matrix instance.
	 */
	Matrix4& Scale(const Vector3<T>& scale){
		//calculate first column
		this->m[0] *= scale[0];
		this->m[1] *= scale[0];
		this->m[2] *= scale[0];
		this->m[3] *= scale[0];

		//calculate second column
		this->m[4] *= scale[1];
		this->m[5] *= scale[1];
		this->m[6] *= scale[1];
		this->m[7] *= scale[1];

		//calculate third column
		this->m[8] *= scale[2];
		this->m[9] *= scale[2];
		this->m[10] *= scale[2];
		this->m[11] *= scale[2];

		//NOTE: 4th column remains unchanged
		return (*this);
	}



	/**
	 * @brief Multiply current matrix by scale matrix.
	 * Multiplies this matrix by Scale matrix from the right (M = M * S).
	 * @param scale - vector of scaling factors in x and y directions, scaing factor in z direction is 1.
	 * @return reference to this Matrix instance.
	 */
	Matrix4& Scale(const Vector2<T>& scale){
		//calculate first column
		this->m[0] *= scale[0];
		this->m[1] *= scale[0];
		this->m[2] *= scale[0];
		this->m[3] *= scale[0];

		//calculate second column
		this->m[4] *= scale[1];
		this->m[5] *= scale[1];
		this->m[6] *= scale[1];
		this->m[7] *= scale[1];

		//NOTE: 3rd and 4th columns remain unchanged
		return (*this);
	}



	/**
	 * @brief Multiply current matrix by scale matrix.
	 * Multiplies this matrix by Scale matrix from the right (M = M * S).
	 * @param x - scaling factor in x directon.
	 * @param y - scaling factor in y directon.
	 * @param z - scaling factor in z directon.
	 * @return reference to this Matrix instance.
	 */
	Matrix4& Scale(T x, T y, T z){
		return this->Scale(Vector3<T>(x, y, z));
	}



	/**
	 * @brief Multiply current matrix by scale matrix.
	 * Multiplies this matrix by Scale matrix from the right (M = M * S).
	 * Scaling factor in z direction is 1.
	 * @param x - scaling factor in x directon.
	 * @param y - scaling factor in y directon.
	 * @return reference to this Matrix instance.
	 */
	Matrix4& Scale(T x, T y){
		return this->Scale(Vector2<T>(x, y));
	}



	/**
	 * @brief Multiply current matrix by scale matrix.
	 * Multiplies this matrix by Scale matrix from the right (M = M * S).
	 * @param scale - scaling factor to be applied in all 3 directon (x, y and z).
	 * @return reference to this Matrix instance.
	 */
	Matrix4& Scale(T scale){
		return this->Scale(Vector3<T>(scale, scale, scale));
	}



	/**
	 * @brief Multiply this matrix by translation matrix.
	 * Multiplies this matrix by Translation matrix from the right (M = M * T)
	 * @param t - translation vector.
	 * @return reference to this matrix object.
	 */
	Matrix4& Translate(const Vector3<T>& t){
		//NOTE: 1st, 2nd and 3rd columns remain unchanged

		//calculate fourth column
		this->m[12] = this->m[0] * t[0] + this->m[4] * t[1] + this->m[8] * t[2] + this->m[12];
		this->m[13] = this->m[1] * t[0] + this->m[5] * t[1] + this->m[9] * t[2] + this->m[13];
		this->m[14] = this->m[2] * t[0] + this->m[6] * t[1] + this->m[10] * t[2] + this->m[14];
		this->m[15] = this->m[3] * t[0] + this->m[7] * t[1] + this->m[11] * t[2] + this->m[15];

		return (*this);
	}



	/**
	 * @brief Multiply this matrix by translation matrix.
	 * Multiplies this matrix by Translation matrix from the right (M = M * T).
	 * Translation only occurs in x-y plane, no translation in z direction,
	 * i.e. z component of translation vector is assumed being 0.
	 * @param t - translation vector.
	 * @return reference to this matrix object.
	 */
	Matrix4& Translate(const Vector2<T>& t){
		//NOTE: 1st, 2nd and 3rd columns remain unchanged

		//calculate fourth column
		this->m[12] = this->m[0] * t[0] + this->m[4] * t[1] + this->m[12];
		this->m[13] = this->m[1] * t[0] + this->m[5] * t[1] + this->m[13];
		this->m[14] = this->m[2] * t[0] + this->m[6] * t[1] + this->m[14];
		this->m[15] = this->m[3] * t[0] + this->m[7] * t[1] + this->m[15];

		return (*this);
	}



	/**
	 * @brief Multiply this matrix by translation matrix.
	 * Multiplies this matrix by Translation matrix from the right (M = M * T).
	 * Translation only occurs in x-y plane, no translation in z direction,
	 * i.e. z component of translation vector is assumed being 0.
	 * @param x - x component of translation vector.
	 * @param y - y component of translation vector.
	 * @return reference to this matrix object.
	 */
	Matrix4& Translate(T x, T y){
		return this->Translate(Vector2<T>(x, y));
	}



	/**
	 * @brief Multiply this matrix by rotation matrix.
	 * Multiplies this matrix by Rotation matrix from the right (M = M * R).
	 * @param q - quaternion, representing the rotation.
	 * @return reference to this matrix object.
	 */
	inline Matrix4& Rotate(const Quaternion<T>& q);//implementation see below



	/**
	 * @brief Multiply this matrix by rotation matrix.
	 * Multiplies this matrix by Rotation matrix from the right (M = M * R).
	 * @param rot - vector, representing the rotation. The vector direction
	 *              defines the axis of rotation, vector length defines
	 *              the angle of rotation in radians.
	 * @return reference to this matrix object.
	 */
	inline Matrix4& Rotate(const Vector3<T>& rot){
		return this->Rotate(Quaternion<T>(rot));
	}



	/**
	 * @brief Multiply this matrix by rotation matrix.
	 * Multiplies this matrix by Rotation matrix from the right (M = M * R).
	 * Rotation is done around (0, 0, 1) axis by given number of radians.
	 * Positive direction of rotation is determined by a right-hand rule.
	 * @param rot - the angle of rotation in radians.
	 * @return reference to this matrix object.
	 */
	inline Matrix4& Rotate(T rot){
		return this->Rotate(Vector3<T>(0, 0, rot));
	}


	
#ifdef DEBUG
	friend std::ostream& operator<<(std::ostream& s, const Matrix4<T>& mat){
		s << "\n";
		s << "/" << mat[0][0] << " " << mat[1][0] << " " << mat[2][0] << " " << mat[3][0] << "\\" << std::endl;
		s << "|" << mat[0][1] << " " << mat[1][1] << " " << mat[2][1] << " " << mat[3][1] << "|" << std::endl;
		s << "|" << mat[0][2] << " " << mat[1][2] << " " << mat[2][2] << " " << mat[3][2] << "|" << std::endl;
		s << "\\" << mat[0][3] << " " << mat[1][3] << " " << mat[2][3] << " " << mat[3][3] << "/";
		return s;
	};
#endif
};//~class Matrix4



//===============================
//
//
//      Quaternion class
//
//
//===============================
/**
 * @brief Quaternion template class.
 */
template <typename T> class Quaternion{
public:
	/**
	 * @brief x component.
	 */
	T x;

	/**
	 * @brief y component.
	 */
	T y;
	
	/**
	 * @brief z component.
	 */
	T z;

	/**
	 * @brief w component.
	 */
	T w;



	/**
	 * @brief Create quaternion with given components.
	 * @param qx - x component.
	 * @param qy - y component.
	 * @param qz - z component.
	 * @param qw - w component.
	 */
	Quaternion(T qx, T qy, T qz, T qw) :
			x(qx),
			y(qy),
			z(qz),
			w(qw)
	{}



	/**
	 * @brief Construct rotation quaternion.
	 * Constructs a quaternion representing rotation (unit quaternion).
	 * Rotation is given by 3 dimensional vector, whose direction defines the
	 * axis about which rotation is done and its magnitude defines the angle of
	 * rotation in radians.
	 * @param axis - vector which defines the rotation.
	 */
	//this constructor creates unit quaternion of a rotation around the axis by |axis| radians
	Quaternion(const Vector3<T>& axis){
		T mag = axis.Magnitude();//magnitude is a rotation angle
		if(mag != 0){
			Vector3<T> a = axis;
			a /= mag;//normalize axis
			this->InitRot(a.x, a.y, a.z, mag);
		}else
			this->Identity();
	}



	/**
	 * @brief Default constructor.
	 * Note, that it does not initialize quaternion components,
	 * right after creation the components are undefined.
	 */
	// A default constructor
	inline Quaternion(){}



	/**
	 * @brief Complex conjugate of this quaternion.
	 * Note, complex conjugate of quaternion (x, y, z, w) is (-x, -y, -z, w).
	 * @return quaternion instance which is a complex conjugate of this quaternion.
	 */
	//"complex conjugate of" operator
	inline Quaternion operator!()const{
		return Quaternion(-this->x, -this->y, -this->z, this->w);
	}



	inline Quaternion& operator+=(const Quaternion& q){
		this->x += q.x;
		this->y += q.y;
		this->z += q.z;
		this->w += q.w;
		return (*this);
	}



	inline Quaternion operator+(const Quaternion& q)const{
		return Quaternion(this->x + q.x, this->y + q.y, this->z + q.z, this->w + q.w);
	}



	inline Quaternion& operator=(const Quaternion& q){
		this->x = q.x;
		this->y = q.y;
		this->z = q.z;
		this->w = q.w;
		return (*this);
	}



	/**
	 * @brief Multiply by scalar and assign.
	 * Multiplies this quaternion by scalar and assigns the result to this quaternion instance.
	 * @param s - scalar value to multiply by.
	 * @return reference to this quaternion instance.
	 */
	inline Quaternion& operator*=(T s){
		this->x *= s;
		this->y *= s;
		this->z *= s;
		this->w *= s;
		return (*this);
	}



	/**
	 * @brief Multiply by scalar.
	 * @param s - scalar value to multiply by.
	 * @return resulting quaternion instance.
	 */
	inline Quaternion operator*(T s)const{
		return (Quaternion(*this) *= s);
	}



	/**
	 * @brief Divide by scalar and assign.
	 * Divide this quaternion by scalar and assigns the result to this quaternion instance.
	 * @param s - scalar value to divide by.
	 * @return reference to this quaternion instance.
	 */
	inline Quaternion& operator/=(T s){
		this->x /= s;
		this->y /= s;
		this->z /= s;
		this->w /= s;
		return (*this);
	}



	/**
	 * @brief Divide by scalar.
	 * @param s - scalar value to divide by.
	 * @return resulting quaternion instance.
	 */
	inline Quaternion operator/(T s)const{
		return (Quaternion(*this) /= s);
	}



	/**
	 * @brief Dot product of quaternions.
	 * Dot product of two quaternions (x1, y1, z1, w1) and
	 * (x2, y2, z2, w2) is a scalar calculated as follows
	 * x1 * x2 + y1 * y2 + z1 * z2 + w1 * w2
	 * @return result of the dot product.
	 */
	//dot product of quaternions
	inline T operator*(const Quaternion& q)const{
		return this->x * q.x + this->y * q.y + this->z * q.z + this->w * q.w;
	}



	/**
	 * @brief Multiply by quaternion and assign.
	 * Multiplies this quaternion by another quaternion from the right
	 * (quaternions multiplication is not associative) and assignes the
	 * result to this quaternion instance.
	 * @param q - quaternion to multiply by.
	 * @return reference to this quaternion instance.
	 */
	//multiplication of quaternions
	Quaternion& operator%=(const Quaternion& q){
		T a = (this->w + this->x) * (q.w + q.x);
		T b = (this->z - this->y) * (q.y - q.z);
		T c = (this->x - this->w) * (q.y + q.z);
		T d = (this->y + this->z) * (q.x - q.w);
		T e = (this->x + this->z) * (q.x + q.y);
		T f = (this->x - this->z) * (q.x - q.y);
		T g = (this->w + this->y) * (q.w - q.z);
		T h = (this->w - this->y) * (q.w + q.z);

		this->x = a - (e + f + g + h) * 0.5f;
		this->y = -c + (e - f + g - h) * 0.5f;
		this->z = -d + (e - f - g + h) * 0.5f;
		this->w = b + (-e - f + g + h) * 0.5f;
		return (*this);
	}



	/**
	 * @brief Multiply by quaternion.
	 * Multiplies this quaternion by another quaternion from the right
	 * (quaternions multiplication is not associative).
	 * @param q - quaternion to multiply by.
	 * @return resulting quaternion instance.
	 */
	//multiplication of quaternions
	inline Quaternion operator%(const Quaternion& q)const{
		return (Quaternion(*this) %= q);
	}



	/**
	 * @brief Initialize with identity quaternion.
	 * Note, identity quaternion is (0, 0, 0, 1).
	 * @return reference to this quaternion instance.
	 */
	inline Quaternion& Identity(){
		this->x = T(0);
		this->y = T(0);
		this->z = T(0);
		this->w = T(1);
		return *this;
	}



	/**
	 * @brief Complex conjugate this quaternion.
	 * Note, complex conjugate of quaternion (x, y, z, w) is (-x, -y, -z, w).
	 * @return reference to this quaternion instance.
	 */
	//Complex conjugate
	inline Quaternion& Conjugate(){
		*this = !(*this);
		return *this;
	}



	/**
	 * @brief Negate this quaternion.
	 * Note, negating quaternion means changing the sign of its every component.
	 * @return reference to this quaternion instance.
	 */
	inline Quaternion& Negate(){
		this->x = -this->x;
		this->y = -this->y;
		this->z = -this->z;
		this->w = -this->w;
		return *this;
	}



	/**
	 * @brief Calculate power 2 from quaternion magnitude.
	 * @return power 2 from magnitude.
	 */
	//returns the magnitude^2 of this quaternion
	inline T MagPow2()const{
		return (*this) * (*this);
	}



	/**
	 * @brief Calculate quaternion magnitude.
	 * @return quaternion magnitude.
	 */
	inline T Magnitude()const{
		return T(sqrt(this->MagPow2()));
	}



	/**
	 * @brief Normalize quaternion.
	 * Note, after normalization, the quaternion becomes a unit quaternion.
	 * @return reference to this quaternion instance.
	 */
	inline Quaternion& Normalize(){
		return (*this) /= Magnitude();
	}



	//TODO: consider removing this function moving its functional to corresponding constructor.
	//Initialize this with rotation unit quaternion from axis (normalized) and an angle
	inline void InitRot(T xx, T yy, T zz, T angle){
		T sina2 = T(sin(angle / 2));
		this->w = T(cos(angle / 2));
		this->x = xx * sina2;
		this->y = yy * sina2;
		this->z = zz * sina2;
	}



	//TODO:consider removing this function
	//multiply this quaternion by unit rotation quaternion
	//from the left
	//TODO: check how this function relates with rotation matrixes multiplication (left-right)
	//      need only "mult from the right" function
	Quaternion& RotateLeft(Vector3<T> axis, T angle){
		Quaternion r;
		r.InitRot(axis.x, axis.y, axis.z, angle);
		return (*this) = r % (*this);
	}



	//Create 4x4 OpenGL like rotation matrix from this quaternion
	//ARGS: m - matrix to fill
	//RETURNS: return a reference to m
	//TODO: move this functionality to  Matrix4::Matrix4(const Quaternion& q), i.e. create a contructor
	Matrix4<T>& CreateMatrix4(Matrix4<T>& m)const{
		// After about 300 trees murdered and 20 packs of chalk depleted, the
		// mathematicians came up with these equations for a quaternion to matrix converion:
		//   /  1-(2y^2+2z^2)   2xy+2zw         2xz-2yw         0   \T
		// M=|  2xy-2zw         1-(2x^2+2z^2)   2zy+2xw         0   |
		//   |  2xz+2yw         2yz-2xw         1-(2x^2+2y^2)   0   |
		//   \  0               0               0               1   /

		//First column
		m[0][0] = T(1) - T(2) * ( Pow2(this->y) + Pow2(this->z) );
		m[0][1] = T(2) * (this->x * this->y + this->z * this->w);
		m[0][2] = T(2) * (this->x * this->z - this->y * this->w);
		m[0][3] = T(0);

		//Second column
		m[1][0] = T(2) * (this->x * this->y - this->z * this->w);
		m[1][1] = T(1) - T(2) * ( Pow2(this->x) + Pow2(this->z) );
		m[1][2] = T(2) * (this->z * this->y + this->x * this->w);
		m[1][3] = T(0);

		//Third column
		m[2][0] = T(2) * (this->x * this->z + this->y * this->w);
		m[2][1] = T(2) * (this->y * this->z - this->x * this->w);
		m[2][2] = T(1) - T(2) * ( Pow2(this->x) + Pow2(this->y) );
		m[2][3] = T(0);

		//Fourth column
		m[3][0] = T(0);
		m[3][1] = T(0);
		m[3][2] = T(0);
		m[3][3] = T(1);
		return m;
	}



	//--||--||--
	Matrix4<T> ToMatrix4()const{
		Matrix4<T> m;
		this->CreateMatrix4(m);
		return m;
	}


	
	//Spherical linear interpolation.
	//This quaternion = SLERP(q1,q2,t), t from [0;1].
	//SLERP(q1,q2,t) = q1*sin((1-t)*alpha)/sin(alpha)+q2*sin(t*alpha)/sin(alpha),
	//where cos(alpha) = (q1,q2) (dot product of normalized quaternions q1 and q2).
	//It is assumed that quaternions are normalized!
	void Slerp(const Quaternion& q1, const Quaternion& q2, T t){
		//Since quaternions are normalized the cosine of the angle alpha
		//between quaternions are equal to their dot product.
		T cosalpha = q1 * q2;

		//If the dot product is less than 0, the angle alpha between quaternions
		//is greater than 90 degrees. Then we negate second quaternion to make alpha
		//be less than 90 degrees. It is possible since normalized quaternions
		//q and -q represent the same rotation!
		if(cosalpha < T(0)){
			//Negate the second quaternion and the result of the dot product (i.e. cos(alpha))
			q2.Negate();
			cosalpha = -cosalpha;
		}

		//interpolation done by the following general formula:
		//RESULT=q1*sc1(t)+q2*sc2(t).
		//Where sc1,sc2 called interpolation scales.
		T sc1, sc2;//Define variables for scales for interpolation

		//Check if the angle alpha between the 2 quaternions is big enough
		//to make SLERP. If alpha is small then we do a simple linear
		//interpolation between quaternions instead of SLERP!
		//It is also used to avoid divide by zero since sin(0)=0 !
		//We made threshold for cos(alpha)>0.9f (if cos(alpha)==1 then alpha=0).
		if(cosalpha > T(0.9f)){
			//Get the angle alpha between the 2 quaternions, and then store the sin(alpha)
			T alpha = T(acos(cosalpha));
			T sinalpha = T(sin(alpha));

			//Calculate the scales for q1 and q2, according to the angle and it's sine value
			sc1 = T( sin((1 - t) * alpha) / sinalpha );
			sc2 = T( sin(t * alpha) / sinalpha );
		}else{
			sc1 = (1 - t);
			sc2 = t;
		}

		// Calculate the x, y, z and w values for the interpolated quaternion.
		(*this) = q1 * sc1 + q2 * sc2;
	}

#ifdef DEBUG  
	friend std::ostream& operator<<(std::ostream& s, const Quaternion<T>& quat){
		s << "(" << quat.x << ", " << quat.y << ", " << quat.z << ", " << quat.w << ")";
		return s;
	}
#endif  
};//~class Quaterion



//===============================================
//
//       inline functions implementation
//
//===============================================

template <class T> inline Vector2<T>::Vector2(const Vector3<T>& vec){
	this->operator=(vec);
}



template <class T> inline Vector2<T>& Vector2<T>::operator=(const Vector3<T>& vec){
	this->x = vec.x;
	this->y = vec.y;
	return (*this);
}



template <class T> inline Vector3<T>& Vector3<T>::operator=(const Vector2<T>& vec){
	this->x = vec.x;
	this->y = vec.y;
	this->z = 0;
	return (*this);
}



template <class T> inline Vector3<T>& Vector3<T>::operator+=(const Vector2<T>& vec){
	this->x += vec.x;
	this->y += vec.y;
	return (*this);
}



template <class T> inline Vector2<T> Vector2<T>::operator+(const Vector3<T>& vec)const{
	return Vector2<T>(
				this->x + vec.x,
				this->y + vec.y
			);
}



template <class T> inline Vector2<T> Vector2<T>::operator-(const Vector3<T>& vec)const{
	return Vector2<T>(
				this->x - vec.x,
				this->y - vec.y
			);
}



template <class T> inline Matrix4<T>& Matrix4<T>::Rotate(const Quaternion<T>& q){
	Matrix4<T> rm;
	q.CreateMatrix4(rm);
	this->RightMultMatrix(rm);
	return (*this);
}



template <class T> inline Vector3<T>& Vector3<T>::Rotate(const Quaternion<T>& q){
	*this = q.ToMatrix4() * (*this);
	return *this;
}



template <class T> class Rectangle2{
	Vector2<T> lb; //Left-Bottom
	Vector2<T> rt; //Right-Top
public:
	
	inline Rectangle2(){}
	
	Rectangle2(T left, T top, T right, T bottom) :
			lb(left, bottom),
			rt(right, top)
	{}

	Rectangle2(Vector2<T> leftBottom, Vector2<T> rightTop) :
			lb(leftBottom),
			rt(rightTop)
	{}
	
	inline void Set(T left, T top, T right, T bottom){
		this->lb = Vector2<T>(left, bottom);
		this->rt = Vector2<T>(right, top);
	}
	
	inline Vector2<T> Center()const{
		return (this->lb + this->rt) / 2;
	}

	inline void SetCenter(const Vector2<T>& vec){
		Vector2<T> offset = vec - Center();
		this->operator +=(offset);
	}

	bool IsIn(const Vector2<T>& vec)const{
		if(this->Left() <= this->Right()){
			if(this->Bottom() <= this->Top()){
				return vec.x <= this->Right() &&
							vec.x >= this->Left() &&
							vec.y >= this->Bottom() &&
							vec.y <= this->Top();
			}else{
				return vec.x <= this->Right() &&
							vec.x >= this->Left() &&
							vec.y <= this->Bottom() &&
							vec.y >= this->Top();
			}
		}else{
			if(this->Bottom() <= this->Top()){
				return vec.x >= this->Right() &&
							vec.x <= this->Left() &&
							vec.y >= this->Bottom() &&
							vec.y <= this->Top();
			}else{
				return vec.x >= this->Right() &&
							vec.x <= this->Left() &&
							vec.y <= this->Bottom() &&
							vec.y >= this->Top();
			}
		}
	}
	
	inline Vector2<T> Extent()const{
		return this->Size()/2;
	}

	inline Vector2<T> Size()const{
		return this->rt - this->lb;
	}
	
	inline Rectangle2& operator=(const Rectangle2<T>&  rect){
		this->rt = rect.rt;
		this->lb = rect.lb;
		return *this;
	}

	inline Rectangle2& operator+=(const Vector2<T>& vec){
		this->rt += vec;
		this->lb += vec;
		return (*this);
	}

	inline Rectangle2 operator*(T num){
		return Rectangle(this->lb * num, this->rt * num);
	}

	inline T& LeftBottom(){
		//TODO: return min out of this->lb.x and this->rt.x
		return this->lb;
	}

	inline const T& LeftBottom()const{
		//TODO: return min out of this->lb.x and this->rt.x
		return this->lb;
	}

	inline T& RightTop(){
		//TODO: return min out of this->lb.x and this->rt.x
		return this->rt;
	}

	inline const T& RightTop()const{
		//TODO: return min out of this->lb.x and this->rt.x
		return this->rt;
	}

	inline T& Left(){
		//TODO: return min out of this->lb.x and this->rt.x
		return this->lb.x;
	}

	inline const T& Left()const{
		return this->lb.x;
	}

	inline T& Top(){
		return this->rt.y;
	}

	inline const T& Top()const{
		return this->rt.y;
	}

	inline T& Right(){
		return this->rt.x;
	}

	inline const T& Right()const{
		return this->rt.x;
	}

	inline T& Bottom(){
		return this->lb.y;
	}

	inline const T& Bottom()const{
		return this->lb.y;
	}
	
	inline T Width()const{
		return this->Right() - this->Left();
	}
	
	inline T Height()const{
		return this->Top() - this->Bottom();
	}
};//~class Rectangle2



//
//
// Some convenient typedefs
//
//

typedef Vector2<int> Vec2i;
STATIC_ASSERT(sizeof(Vec2i) == sizeof(int) * 2)
typedef Vector2<unsigned> Vec2ui;
STATIC_ASSERT(sizeof(Vec2ui) == sizeof(unsigned) * 2)
typedef Vector2<float> Vec2f;
STATIC_ASSERT(sizeof(Vec2f) == sizeof(float) * 2)
typedef Vector2<double> Vec2d;
STATIC_ASSERT(sizeof(Vec2d) == sizeof(double) * 2)

typedef Vector3<float> Vec3f;
STATIC_ASSERT(sizeof(Vec3f) == sizeof(float) * 3)
typedef Vector3<double> Vec3d;
STATIC_ASSERT(sizeof(Vec3d) == sizeof(double) * 3)

typedef Vector4<float> Vec4f;
STATIC_ASSERT(sizeof(Vec4f) == sizeof(float) * 4)
typedef Vector4<double> Vec4d;
STATIC_ASSERT(sizeof(Vec4d) == sizeof(double) * 4)

typedef Matrix4<float> Matr4f;
typedef Matrix4<double> Matr4d;

typedef Quaternion<float> Quatf;
typedef Quaternion<double> Quatd;

typedef Rectangle2<float> Rect2f;
typedef Rectangle2<double> Rect2d;
typedef Rectangle2<int> Rect2i;

}//~namespace
